/*
 * SPDX-FileCopyrightText: 2023 ML!PA Consulting GmbH
 * SPDX-License-Identifier: LGPL-2.1-only
 */

/**
 * @ingroup     cpu_samd5x
 * @{
 *
 * @file
 * @brief       Implementation of the CAN controller driver
 *
 * @author      Firas Hamdi <firas.hamdi@ml-pa.com>
 * @}
 */

#include "can/can.h"
#include <assert.h>
#include <stdint.h>
#include <string.h>

#include "can/device.h"
#include "periph/can.h"
#include "periph/gpio.h"
#include "pm_layered.h"

#define ENABLE_DEBUG 0
#include "debug.h"

/**
 * @brief Value from SAMD5x/E5x Family datasheet, Tables 39-13 and 39-17
 */
#define CANDEV_SAMD5X_CLASSIC_FILTER 0x02
/**
 * @brief Set to 1 to access the internal loopback mode.
 * Check SAMD5x/E5x Family datasheet, Figure 39-4
 */
#define CANDEV_SAMD5X_INTERNAL_LOOPBACK 0

/**
 * @brief Specific configuration of the CAN filter
 */
enum {
    CANDEV_SAMD5X_FILTER_DISABLE = 0x00,
    CANDEV_SAMD5X_FILTER_RX_FIFO_0,
    CANDEV_SAMD5X_FILTER_RX_FIFO_1
};

/**
 * @brief Configuration of how to handle frames not matching the CAN filters
 */
enum {
    /* Direct frames not matching any CAN filters applied to Rx FIFO 0 */
    CAN_ACCEPT_RX_FIFO_0 = 0x00,
    /* Direct frames not matching any CAN filters applied to Rx FIFO 1 */
    CAN_ACCEPT_RX_FIFO_1,
    /* Reject all frames not matching any CAN filters applied */
    CAN_REJECT
};

typedef enum {
    MODE_INIT,
    MODE_LOOPBACK,
    MODE_MONITOR,
} can_mode_t;

/* Used to handle interrupts generated by the CAN controller 0 */
static can_t *_can_0;
/* Used to handle interrupts generated by the CAN controller 1 */
static can_t *_can_1;

static int _init(candev_t *candev);
static int _send(candev_t *candev, const struct can_frame *frame);
static int _set_filter(candev_t *candev, const struct can_filter *filter);
static int _remove_filter(candev_t *candev, const struct can_filter *filter);
static int _set(candev_t *candev, canopt_t opt, void *value, size_t value_len);
static void _isr(candev_t *candev);

static int _set_mode(Can *can, can_mode_t mode);

static const candev_driver_t candev_samd5x_driver = {
    .init = _init,
    .send = _send,
    .set_filter = _set_filter,
    .remove_filter = _remove_filter,
    .set = _set,
    .isr = _isr,
};

/* Values taken from SAMD5x/E5x datasheet section 39.8.8 */
static const struct can_bittiming_const bittiming_const = {
    .tseg1_min = 1,
    .tseg1_max = 256,
    .tseg2_min = 1,
    .tseg2_max = 128,
    .sjw_max = 128,
    .brp_min = 1,
    .brp_max = 512,
    .brp_inc = 1,
};

/* since this driver is sam only its safe to assume that 0 is an undefined pin */
static bool _pin_is_valid_and_defined(gpio_t pin)
{
     return (pin) && gpio_is_valid(pin);
}

static int _power_on(can_t *dev)
{
    /* CAN required CLK_CANx_APB and GCLK_CANx to be running and will not
     * request any clock by itself. We can ensure both clocks to be running
     * by preventing the MCU from entering IDLE state.
     *
     * The SAMD5x/SAME5x Family Data Sheet says in Section
     * "39.6.9 Sleep Mode Operation" says:
     *
     * > The CAN can be configured to operate in any idle sleep mode. The CAN
     * > cannot operate in Standby sleep mode.
     * >
     * > [...]
     * >
     * > To leave low power mode, CLK_CANx_APB and GCLK_CANx must be active
     * > before writing CCCR.CSR to '0'. The CAN will acknowledge this by
     * > resetting CCCR.CSA = 0. Afterwards, the application can restart CAN
     * > communication by resetting bit CCCR.INIT.
     *
     * tl;dr: At most SAM0_PM_IDLE is allowed while not shutting down the CAN
     * controller, but even that will pause communication (including RX).
     */
    if (IS_USED(MODULE_PM_LAYERED)) {
        pm_block(SAM0_PM_IDLE);
    }

    if (_pin_is_valid_and_defined(dev->conf->enable_pin)) {
        gpio_write(dev->conf->enable_pin, !dev->conf->enable_pin_active_low);
    }

    if (dev->conf->can == CAN0) {
        DEBUG_PUTS("CAN0 controller is used");
        MCLK->AHBMASK.reg |= MCLK_AHBMASK_CAN0;
    }
    else if (dev->conf->can == CAN1) {
        DEBUG_PUTS("CAN1 controller is used");
        MCLK->AHBMASK.reg |= MCLK_AHBMASK_CAN1;
    }
    else {
        DEBUG_PUTS("Unsupported CAN channel");
        assert(0);
    }

    return 0;
}

static int _power_off(can_t *dev)
{
    if (IS_USED(MODULE_PM_LAYERED)) {
        pm_unblock(SAM0_PM_IDLE);
    }

    if (dev->conf->can == CAN0) {
        DEBUG_PUTS("CAN0 controller is used");
        MCLK->AHBMASK.reg &= ~MCLK_AHBMASK_CAN0;
    }
    else if (dev->conf->can == CAN1) {
        DEBUG_PUTS("CAN1 controller is used");
        MCLK->AHBMASK.reg &= ~MCLK_AHBMASK_CAN1;
    }
    else {
        DEBUG_PUTS("Unsupported CAN channel");
        return -1;
    }

    if (_pin_is_valid_and_defined(dev->conf->enable_pin)) {
        gpio_write(dev->conf->enable_pin, dev->conf->enable_pin_active_low);
    }

    return 0;
}

static void _enter_init_mode(Can *can)
{
    can->CCCR.reg |= CAN_CCCR_INIT;
    while (!(can->CCCR.reg & CAN_CCCR_INIT)) {}
    DEBUG_PUTS("Device in init mode");
}

static void _exit_init_mode(Can *can)
{
    if (can->CCCR.reg & CAN_CCCR_INIT) {
        can->CCCR.reg &= ~CAN_CCCR_INIT;
    }

    while (can->CCCR.reg & CAN_CCCR_INIT) {}
    DEBUG_PUTS("Device out of init mode");
}

static int _set_mode(Can *can, can_mode_t can_mode)
{
    switch (can_mode) {
        case MODE_INIT:
            _enter_init_mode(can);
            can->CCCR.reg |= CAN_CCCR_CCE;
            break;
        case MODE_LOOPBACK:
            DEBUG_PUTS("test mode");
            _enter_init_mode(can);
            /* CCCR.TEST and CCCR.MON can be set only when CCCR.INIT and CCCR.CCE are set */
            can->CCCR.reg |= CAN_CCCR_CCE;
            can->CCCR.reg |= CAN_CCCR_TEST;
            can->TEST.reg |= CAN_TEST_LBCK;
#if IS_ACTIVE(CANDEV_SAMD5X_INTERNAL_LOOPBACK)
            can->CCCR.reg |= CAN_CCCR_MON;
#endif
            _exit_init_mode(can);
            break;
        case MODE_MONITOR:
            DEBUG_PUTS("monitor mode");
            _enter_init_mode(can);
            /* CCCR.TEST and CCCR.MON can be set only when CCCR.INIT and CCCR.CCE are set */
            can->CCCR.reg |= CAN_CCCR_CCE;
            can->CCCR.reg |= CAN_CCCR_MON;
            _exit_init_mode(can);
            break;
        default:
            DEBUG_PUTS("Unsupported mode");
            return -1;
    }

    return 0;
}

static void _setup_clock(can_t *dev)
{
    int pchid = 0;
    if (dev->conf->can == CAN0){
        pchid = CAN0_GCLK_ID;
    }
    else if (dev->conf->can == CAN1){
        pchid = CAN1_GCLK_ID;
    }
    else {
        /* only CAN0 and CAN1 supported. When ported to MCUs with more
         * CAN controllers, this code needs to be adapted */
        DEBUG_PUTS("CAN channel not supported");
        assert(0);
        return;
    }

    uint32_t pchctrl = GCLK->PCHCTRL[pchid].reg;

    /* disable */
    GCLK->PCHCTRL[pchid].reg = pchctrl & (~GCLK_PCHCTRL_CHEN);
    do {
        pchctrl = GCLK->PCHCTRL[pchid].reg;
    } while (pchctrl & GCLK_PCHCTRL_CHEN);

    /* setup */
    pchctrl = GCLK_PCHCTRL_GEN(dev->conf->gclk_src);
    GCLK->PCHCTRL[pchid].reg = pchctrl;

    /* enable */
    pchctrl |= GCLK_PCHCTRL_CHEN;
    GCLK->PCHCTRL[pchid].reg = pchctrl;
    do {
        pchctrl = GCLK->PCHCTRL[pchid].reg;
    } while (!(pchctrl & GCLK_PCHCTRL_CHEN));
}

static void _set_bit_timing(can_t *dev)
{
    struct can_bittiming *bittiming = &dev->candev.bittiming;
    assert(bittiming->sjw >= 1);
    assert(bittiming->phase_seg2 >= 1);
    assert(bittiming->phase_seg1 + bittiming->prop_seg >= 1);
    assert(bittiming->brp >= 1);

    DEBUG("bitrate=%" PRIu32 ", sample_point=%" PRIu32 ", brp=%" PRIu32 ", prop_seg=%" PRIu32
          ", phase_seg1=%" PRIu32 ", phase_seg2=%" PRIu32 ", sjw=%" PRIu32 "\n",
          bittiming->bitrate, bittiming->sample_point,
          bittiming->brp, bittiming->prop_seg,
          bittiming->phase_seg1, bittiming->phase_seg2,
          bittiming->sjw);

    /* Set bit timing */
    uint32_t nbtp = CAN_NBTP_NTSEG2(bittiming->phase_seg2 - 1)
                    | CAN_NBTP_NTSEG1(bittiming->phase_seg1 + bittiming->prop_seg - 1)
                    | CAN_NBTP_NBRP(bittiming->brp - 1)
                    | CAN_NBTP_NSJW(bittiming->sjw - 1);

    dev->conf->can->NBTP.reg = nbtp;
}

static void _set_tx_fifo_data_size(can_t *dev, uint8_t size) {
    assert(size < 0x8);
    dev->conf->can->TXESC.reg |= CAN_TXESC_TBDS(size);
}

static void _set_rx_buffer_data_size(can_t *dev, uint8_t size) {
    assert(size < 0x8);
    dev->conf->can->RXESC.reg |= CAN_RXESC_RBDS(size);
}

static void _set_rx_fifo_0_data_size(can_t *dev, uint8_t size) {
    assert(size < 0x8);
    dev->conf->can->RXESC.reg |= CAN_RXESC_F0DS(size);
}

static void _set_rx_fifo_1_data_size(can_t *dev, uint8_t size) {
    assert(size < 0x8);
    dev->conf->can->RXESC.reg |= CAN_RXESC_F1DS(size);
}

static void _set_can_pins(can_t *dev)
{
    assert(dev->conf->tx_pin != GPIO_UNDEF);
    assert(dev->conf->rx_pin != GPIO_UNDEF);

    gpio_init(dev->conf->tx_pin, GPIO_OUT);
    gpio_init(dev->conf->rx_pin, GPIO_IN);

    if (dev->conf->can == CAN0) {
        gpio_init_mux(dev->conf->tx_pin, GPIO_MUX_I);
        gpio_init_mux(dev->conf->rx_pin, GPIO_MUX_I);
    }
    else if (dev->conf->can == CAN1) {
        gpio_init_mux(dev->conf->tx_pin, GPIO_MUX_H);
        gpio_init_mux(dev->conf->rx_pin, GPIO_MUX_H);
    }
    else {
        DEBUG_PUTS("Unsupported can channel");
    }
}

void candev_samd5x_enter_sleep_mode(candev_t *candev)
{
    can_t *dev = container_of(candev, can_t, candev);

    dev->conf->can->CCCR.reg |= CAN_CCCR_CSR;
    while (!(dev->conf->can->CCCR.reg & CAN_CCCR_CSA)) {}
    DEBUG_PUTS("Device in sleep mode");
}

void candev_samd5x_exit_sleep_mode(candev_t *candev)
{
    can_t *dev = container_of(candev, can_t, candev);

    dev->conf->can->CCCR.reg &= ~CAN_CCCR_CSR;
    while (dev->conf->can->CCCR.reg & CAN_CCCR_CSA) {}
    DEBUG_PUTS("Device out of sleep mode");
}

void candev_samd5x_tdc_control(can_t *dev)
{
    if (dev->tdc_ctrl) {
        DEBUG_PUTS("Enable Transceiver Delay Compensation");
        dev->conf->can->DBTP.reg |= CAN_DBTP_TDC;
    }
    else {
        DEBUG_PUTS("Disable Transceiver Delay Compensation");
        dev->conf->can->DBTP.reg &= ~(CAN_DBTP_TDC);
    }
}

void can_init(can_t *dev, const can_conf_t *conf)
{
    dev->candev.driver = &candev_samd5x_driver;

    struct can_bittiming timing = {
        .bitrate = conf->bitrate ? conf->bitrate : CANDEV_SAMD5X_DEFAULT_BITRATE,
        .sample_point = CANDEV_SAMD5X_DEFAULT_SPT
    };

    uint32_t clk_freq = sam0_gclk_freq(conf->gclk_src);
    can_device_calc_bittiming(clk_freq, &bittiming_const, &timing);

    memcpy(&dev->candev.bittiming, &timing, sizeof(timing));
    dev->conf = conf;

    if (_pin_is_valid_and_defined(conf->enable_pin)) {
        /* In case conf->enable_pin is not initialized aid debugging with a
         * blown assertion */
        assert((uint32_t)conf->enable_pin);
        gpio_init(conf->enable_pin, conf->enable_pin_mode);
        gpio_write(conf->enable_pin, conf->enable_pin_active_low);
    }
}

static void _dump_msg_ram_section(can_t *dev)
{
    puts("start address|\tsize of section");
    printf("SFF filters|\t0x%08" PRIxPTR "|\t%u\n", (uintptr_t)(dev->msg_ram.std_filter),
           (unsigned)(ARRAY_SIZE(dev->msg_ram.std_filter)));
    printf("EFF filters|\t0x%08" PRIxPTR "|\t%u\n", (uintptr_t)(dev->msg_ram.ext_filter),
           (unsigned)(ARRAY_SIZE(dev->msg_ram.ext_filter)));
    printf("Rx FIFO 0|\t0x%08" PRIxPTR " |\t%u\n", (uintptr_t)(dev->msg_ram.rx_fifo_0),
           (unsigned)(ARRAY_SIZE(dev->msg_ram.rx_fifo_0)));
    printf("Rx FIFO 1|\t0x%08" PRIxPTR "|\t%u\n", (uintptr_t)(dev->msg_ram.rx_fifo_1),
           (unsigned)(ARRAY_SIZE(dev->msg_ram.rx_fifo_1)));
    printf("Rx buffer|\t0x%08" PRIxPTR "|\t%u\n", (uintptr_t)(dev->msg_ram.rx_buffer),
           (unsigned)(ARRAY_SIZE(dev->msg_ram.rx_buffer)));
    printf("Tx event FIFO|\t0x%08" PRIxPTR "|\t%u\n", (uintptr_t)(dev->msg_ram.tx_event_fifo),
           (unsigned)(ARRAY_SIZE(dev->msg_ram.tx_event_fifo)));
    printf("Tx buffer|\t0x%08" PRIxPTR "|\t%u\n", (uintptr_t)(dev->msg_ram.tx_buffer),
           (unsigned)(ARRAY_SIZE(dev->msg_ram.tx_buffer)));
}

static int _init(candev_t *candev)
{
    can_t *dev = container_of(candev, can_t, candev);
    int res = 0;

    sam0_gclk_enable(dev->conf->gclk_src);

    _setup_clock(dev);
    _power_on(dev);

    _set_can_pins(dev);

    res = _set_mode(dev->conf->can, MODE_INIT);
    if (res != 0) {
        return -1;
    }

    _set_bit_timing(dev);

    candev_samd5x_tdc_control(dev);

    /*Configure the start addresses of the RAM message sections */
    dev->conf->can->SIDFC.reg = CAN_SIDFC_FLSSA((uintptr_t)(dev->msg_ram.std_filter))
                              | CAN_SIDFC_LSS((size_t)(ARRAY_SIZE(dev->msg_ram.std_filter)));
    dev->conf->can->XIDFC.reg = CAN_XIDFC_FLESA((uintptr_t)(dev->msg_ram.ext_filter))
                              | CAN_XIDFC_LSE((size_t)(ARRAY_SIZE(dev->msg_ram.ext_filter)));
    dev->conf->can->RXF0C.reg = CAN_RXF0C_F0SA((uintptr_t)(dev->msg_ram.rx_fifo_0))
                              | CAN_RXF0C_F0S((size_t)(ARRAY_SIZE(dev->msg_ram.rx_fifo_0)));
    dev->conf->can->RXF1C.reg = CAN_RXF1C_F1SA((uintptr_t)(dev->msg_ram.rx_fifo_1))
                              | CAN_RXF1C_F1S((uintptr_t)(ARRAY_SIZE(dev->msg_ram.rx_fifo_1)));
    dev->conf->can->RXBC.reg = CAN_RXBC_RBSA((size_t)(dev->msg_ram.rx_buffer));
    dev->conf->can->TXEFC.reg = CAN_TXEFC_EFSA((uintptr_t)(dev->msg_ram.tx_event_fifo))
                              | CAN_TXEFC_EFS((size_t)(ARRAY_SIZE(dev->msg_ram.tx_event_fifo)));
    dev->conf->can->TXBC.reg = CAN_TXBC_TBSA((uintptr_t)(dev->msg_ram.tx_buffer))
                              | CAN_TXBC_TFQS((size_t)(ARRAY_SIZE(dev->msg_ram.tx_buffer)));

    /* In the vendor file, the data field size in CanMramTxbe is set to 64 bytes
        although it can be configurable. That's why 64 bytes is used here by default */
    _set_tx_fifo_data_size(dev, CAN_RXESC_F1DS_DATA64_Val);
    /* In the vendor file, the data field size in CanMramRxbe is set to 64 bytes
        although it can be configurable. That's why 64 bytes is used here by default */
    _set_rx_buffer_data_size(dev, CAN_RXESC_RBDS_DATA64_Val);
    /* In the vendor file, the data field size in CanMramRxf0e is set to 64 bytes
        although it can be configurable. That's why 64 bytes is used here by default */
    _set_rx_fifo_0_data_size(dev, CAN_RXESC_F0DS_DATA64_Val);
    /* In the vendor file, the data field size in CanMramRxf1e is set to 64 bytes
        although it can be configurable. That's why 64 bytes is used here by default */
    _set_rx_fifo_1_data_size(dev, CAN_RXESC_F1DS_DATA64_Val);

    if (IS_ACTIVE(ENABLE_DEBUG)) {
        _dump_msg_ram_section(dev);
    }

    uint32_t cccr = dev->conf->can->CCCR.reg;
    cccr &= ~(CAN_CCCR_DAR | CAN_CCCR_TXP | CAN_CCCR_MON);

    if (dev->conf->disable_automatic_retransmission) {
         cccr |= CAN_CCCR_DAR;
    }

    if (dev->conf->enable_transmit_pause) {
        cccr |= CAN_CCCR_TXP;
    }

    if (dev->conf->start_in_monitor_mode) {
        cccr |= CAN_CCCR_MON;
    }

    dev->conf->can->CCCR.reg = cccr;

    /* Reject all remote frames */
    dev->conf->can->GFC.reg = CAN_GFC_RRFE | CAN_GFC_RRFS;

    /* Enable reception interrupts: reception on FIFO0 and FIFO1 */
    uint32_t ie_reg = CAN_IE_RF0NE | CAN_IE_RF1NE;
    /* Enable transmission events interrupts */
    ie_reg |= CAN_IE_TEFNE;
    /* Enable errors interrupts */
    ie_reg |= CAN_IE_PEDE | CAN_IE_PEAE | CAN_IE_BOE | CAN_IE_EWE | CAN_IE_EPE;
    /* write Interrupt enable register */
    dev->conf->can->IE.reg = ie_reg;

    /* Enable the interrupt lines */
    dev->conf->can->ILE.reg = CAN_ILE_EINT0 | CAN_ILE_EINT1;

    /* Enable the peripheral's interrupt */
    if (dev->conf->can == CAN0) {
        NVIC_EnableIRQ(CAN0_IRQn);
        _can_0 = dev;
    }
    else {
        NVIC_EnableIRQ(CAN1_IRQn);
        _can_1 = dev;
    }

    /* Exit initialization mode */
    _exit_init_mode(dev->conf->can);

    return res;
}

static uint8_t _form_message_marker(size_t idx)
{
    const unsigned lowest_nibble = 0x0f;
    static unsigned count;
    count++;
    return (lowest_nibble & idx) | ((count & lowest_nibble) << 4);
}

static int _send(candev_t *candev, const struct can_frame *frame)
{
    /* this assertion ensures the EFF-FLAG is set or the id does not exceed the CAN_SFF_MASK*/
    assert( (frame->can_id & CAN_EFF_FLAG)
            || ((frame->can_id & CAN_SFF_MASK) == (frame->can_id & CAN_EFF_MASK)) );
    can_t *dev = container_of(candev, can_t, candev);

    if (frame->can_dlc > CAN_MAX_DLEN) {
        DEBUG_PUTS("CAN frame payload not supported");
        return -1;
    }
    uint32_t txfqs = dev->conf->can->TXFQS.reg;
    /* Check if the Tx FIFO is full */
    if (txfqs & CAN_TXFQS_TFQF) {
        DEBUG_PUTS("Tx FIFO is full");
        return -1;
    }

    size_t put_idx = (txfqs & CAN_TXFQS_TFQPI_Msk) >> CAN_TXFQS_TFQPI_Pos;
    size_t get_idx = (txfqs & CAN_TXFQS_TFGI_Msk) >> CAN_TXFQS_TFGI_Pos;
    DEBUG("Tx FIFO put index = %u\n", put_idx);
    DEBUG("Tx FIFO get index = %u\n", get_idx);

    uint32_t txbe0 = 0;
    uint32_t txbe1 = 0;
    if (frame->can_id & CAN_EFF_FLAG) {
        DEBUG_PUTS("EFF ID");
        txbe0 = CAN_TXBE_0_ID(frame->can_id & CAN_EFF_MASK) | CAN_TXBE_0_XTD;
    }
    else {
        DEBUG_PUTS("Standard identifier");
        txbe0 = CAN_TXBE_0_ID((frame->can_id & CAN_SFF_MASK) << 18);
    }
    if (frame->can_id & CAN_RTR_FLAG){
        txbe0 |= CAN_TXBE_0_RTR;
    }
    txbe0 |= CAN_TXBE_0_ESI;

    /* Write the prepared word */
    dev->msg_ram.tx_buffer[put_idx].TXBE_0.reg = txbe0;

    /* Prepare second word */
    uint8_t tx_mm = _form_message_marker(put_idx);
    txbe1 = CAN_TXBE_1_DLC(frame->can_dlc) | CAN_TXBE_1_EFC | CAN_TXBE_1_MM(tx_mm);
    /* Write the second word */
    dev->msg_ram.tx_buffer[put_idx].TXBE_1.reg = txbe1;

    memcpy((void *)dev->msg_ram.tx_buffer[put_idx].TXBE_DATA, frame->data, frame->can_dlc);

    /* Request transmission */
    dev->conf->can->TXBAR.reg |= (1 << put_idx);

    /* message marker is more useful to identify this message transmission than mailbox*/
    return tx_mm;
}

static ssize_t _find_filter(can_t *can, const struct can_filter *filter, bool is_std_filter)
{
    ssize_t idx = -1;
    uint32_t msg_id;
    /* Standard filter */
    if (is_std_filter) {
        /* Search for the standard filter in the CAN controller message RAM */
        for (size_t i = 0; i < ARRAY_SIZE(can->msg_ram.std_filter); i++) {
            msg_id = ((can->msg_ram.std_filter[i].SIDFE_0.reg &
                       CAN_SIDFE_0_SFID1_Msk) >> CAN_SIDFE_0_SFID1_Pos);
            if ((filter->can_id & CAN_SFF_MASK) == msg_id ) {
                idx = (ssize_t)i;
                break;
            }
        }
    }
    /* Extended filter */
    else {
        /* Search for the extended filter in the CAN controller message RAM */
        for (size_t i = 0; i < ARRAY_SIZE(can->msg_ram.ext_filter); i++) {
            msg_id = ((can->msg_ram.ext_filter[i].XIDFE_0.reg &
                       CAN_XIDFE_0_EFID1_Msk) >> CAN_XIDFE_0_EFID1_Pos);
            if ((filter->can_id & CAN_EFF_MASK) == msg_id) {
                idx = (ssize_t)i;
                break;
            }
        }
    }

    return idx;
}

static int _set_filter(candev_t *candev, const struct can_filter *filter)
{
    can_t *dev = container_of(candev, can_t, candev);

    ssize_t idx = 0;
    uint8_t mbx = 0;
    switch (filter->target_mailbox) {
        case 0:
            mbx = CANDEV_SAMD5X_FILTER_RX_FIFO_0;
            break;
        case 1 :
            mbx = CANDEV_SAMD5X_FILTER_RX_FIFO_1;
            break;
        default:
            puts("Invalid target mailbox --> Do not apply filter");
            return -1;
    }
    if (filter->can_id & CAN_EFF_FLAG) {
        DEBUG_PUTS("EFF id filter to add in the extended filter section of the message RAM");
        /* Check if the filter already exists */
        idx = _find_filter(dev, filter, false);
        if (idx != -1) {
            DEBUG_PUTS("EFF id filter already exists --> Update it");
        }
        else {
            /* Find a free slot where to save the filter */
            for (idx = 0; (uint16_t)idx < ARRAY_SIZE(dev->msg_ram.ext_filter); idx++) {
                uint32_t xidfe_0 = dev->msg_ram.ext_filter[idx].XIDFE_0.reg;
                if (((xidfe_0 & CAN_XIDFE_0_EFEC_Msk) >> CAN_XIDFE_0_EFEC_Pos) ==
                    CANDEV_SAMD5X_FILTER_DISABLE)
                {
                    DEBUG_PUTS("empty slot");
                    break;
                }
            }
        }

        if (idx == ARRAY_SIZE(dev->msg_ram.ext_filter)) {
            DEBUG_PUTS("Reached maximum capacity of extended filters --> Could not add filter");
            return -1;
        }

        DEBUG("EFF id filter to add at idx = %d\n", idx);
        /* For now, only CLASSIC filters are supported */
        /* avoid multiple writes into volatile memory*/
        uint32_t xidfe_0 = CAN_XIDFE_0_EFEC(mbx) | CAN_XIDFE_0_EFID1(filter->can_id);
        uint32_t xidfe_1 = CAN_XIDFE_1_EFT(CANDEV_SAMD5X_CLASSIC_FILTER)
                         | CAN_XIDFE_1_EFID2(filter->can_mask & CAN_EFF_MASK);

        dev->msg_ram.ext_filter[idx].XIDFE_0.reg |= xidfe_0;
        dev->msg_ram.ext_filter[idx].XIDFE_1.reg |= xidfe_1;
        DEBUG("EFF id filter element N°%d: F0 = 0x%08" PRIxPTR ", F1 = 0x%08" PRIxPTR "\n",
              idx, (uintptr_t)(dev->msg_ram.ext_filter[idx].XIDFE_0.reg),
              (uintptr_t)(dev->msg_ram.ext_filter[idx].XIDFE_1.reg));
        _set_mode(dev->conf->can, MODE_INIT);
        /* Reject all extended frames that are not matching the filters applied */
        dev->conf->can->GFC.reg |= CAN_GFC_ANFE((uint32_t)CAN_REJECT);
        _exit_init_mode(dev->conf->can);
    }
    else {
        DEBUG_PUTS("SFF id filter to add in the standard filter section of the message RAM");
        /* Check if the filter already exists */
        idx = _find_filter(dev, filter, true);
        if (idx != -1) {
            DEBUG_PUTS("SFF id filter already exists --> Update it");
        }
        else {
            /* Find a free slot where to save the filter */
            for (idx = 0; (uint16_t)idx < ARRAY_SIZE(dev->msg_ram.std_filter); idx++) {
                uint32_t sidfe_0 = dev->msg_ram.std_filter[idx].SIDFE_0.reg;
                if (((sidfe_0 & CAN_SIDFE_0_SFEC_Msk) >> CAN_SIDFE_0_SFEC_Pos) ==
                    CANDEV_SAMD5X_FILTER_DISABLE)
                {
                    DEBUG_PUTS("empty slot");
                    break;
                }
            }
        }

        if (idx == ARRAY_SIZE(dev->msg_ram.std_filter)) {
            DEBUG_PUTS("Reached maximum capacity of standard filters --> Could not add filter");
            return -1;
        }

        DEBUG("SFF id filter to add at idx = %d\n", idx);
        /* For now, only CLASSIC filters are supported */
        uint32_t sidfe_0 = CAN_SIDFE_0_SFEC(mbx)
                         | CAN_SIDFE_0_SFT(CANDEV_SAMD5X_CLASSIC_FILTER)
                         | CAN_SIDFE_0_SFID1(filter->can_id & CAN_SFF_MASK)
                         | CAN_SIDFE_0_SFID2(filter->can_mask & CAN_SFF_MASK);
        dev->msg_ram.std_filter[idx].SIDFE_0.reg = sidfe_0;

        DEBUG("SFF id filter element N°%d: S0 = 0x%08" PRIxPTR "\n",
              (int)idx, (uintptr_t)(dev->msg_ram.std_filter[idx].SIDFE_0.reg));
        _set_mode(dev->conf->can, MODE_INIT);
        /* Reject all standard frames that are not matching the filters applied */
        dev->conf->can->GFC.reg |= CAN_GFC_ANFS((uint32_t)CAN_REJECT);
        _exit_init_mode(dev->conf->can);
    }

    return idx;
}

static int _remove_filter(candev_t *candev, const struct can_filter *filter)
{
    can_t *dev = container_of(candev, can_t, candev);

    ssize_t idx = 0;
    if (filter->can_id & CAN_EFF_FLAG) {
        idx = _find_filter(dev, filter, false);
        if (idx != -1) {
            DEBUG("EFF id filter to disable at idx = %d\n", idx);
            dev->msg_ram.ext_filter[idx].XIDFE_0.reg &= ~CAN_XIDFE_0_EFEC_Msk;
        }
        else {
            DEBUG_PUTS("Filter not found");
            return -1;
        }
    }
    else {
        idx = _find_filter(dev, filter, true);
        if (idx != -1) {
            DEBUG("SFF id filter to disable at idx = %d\n", idx);
            dev->msg_ram.std_filter[idx].SIDFE_0.reg &= ~CAN_SIDFE_0_SFEC_Msk;
        }
        else {
            DEBUG_PUTS("Filter not found");
            return -1;
        }
    }

    return idx;
}

static int _set(candev_t *candev, canopt_t opt, void *value, size_t value_len)
{
    can_t *dev = container_of(candev, can_t, candev);
    int res = 0;

    switch (opt) {
        case CANOPT_BITTIMING:
            if (value_len < sizeof(struct can_bittiming)) {
                return -1;
            }
            else {
                memcpy(&candev->bittiming, value, sizeof(struct can_bittiming));
                uint32_t clk_freq = sam0_gclk_freq(dev->conf->gclk_src);
                can_device_calc_bittiming(clk_freq, &bittiming_const, &candev->bittiming);
                res = _init(candev);
                if (res == 0) {
                    res = sizeof(candev->bittiming);
                }
                else {
                    return -1;
                }
            }
            break;
        case CANOPT_RX_FILTERS:
            if (value_len < sizeof(struct can_filter)) {
                return -1;
            }
            else {
                res = _set_filter(candev, value);
                if (res >= 0) {
                    res = sizeof(struct can_filter);
                }
                else {
                    return -1;
                }
            }
            break;
        case CANOPT_STATE:
            if (value_len < sizeof(canopt_state_t)) {
                return -1;
            }
            else {
                switch (*((canopt_state_t *)value)) {
                    case CANOPT_STATE_OFF:
                        res = _power_off(dev);
                        if (res == 0) {
                            res = sizeof(canopt_state_t);
                        }
                        else {
                            return -1;
                        }
                        break;
                    case CANOPT_STATE_ON:
                        res = _power_on(dev);
                        if (res == 0) {
                            res = sizeof(canopt_state_t);
                        }
                        else {
                            return -1;
                        }
                        break;
                    case CANOPT_STATE_LOOPBACK:
                        res = _set_mode(dev->conf->can, MODE_LOOPBACK);
                        if (res == 0) {
                            res = sizeof(canopt_state_t);
                        }
                        else {
                            return -1;
                        }
                        break;
                    case CANOPT_STATE_LISTEN_ONLY:
                        res = _set_mode(dev->conf->can, MODE_MONITOR);
                        if (res == 0) {
                            res = sizeof(canopt_state_t);
                        }
                        else {
                            return -1;
                        }
                        break;
                    default:
                        break;
                }
            }
        default:
            break;
    }

    return res;
}

static void _mcan_hdr_can_frame(struct can_frame * frame, uint32_t fe_r0, uint32_t fe_r1)
{
    /* while fifo element R0 and R1 in rx and tx are not the same
     * the most relevant bits of r0 and r1 are shared between the different
     * fifo element types this using the TXEFE definition to parse them */
    canid_t canid = 0;
    if (fe_r0 & CAN_TXEFE_0_XTD) {
        canid |= CAN_EFF_FLAG;
        canid |= (fe_r0 & CAN_TXEFE_0_ID_Msk) >> CAN_TXEFE_0_ID_Pos;
    }
    else {
        /* 11 bit can id are stored in bit [28:18] of the ID field */
        canid |= (fe_r0 & CAN_TXEFE_0_ID_Msk) >> CAN_TXEFE_0_ID_Pos >> 18;
    }

    if (fe_r0 & CAN_TXEFE_0_RTR) {
        canid |= CAN_RTR_FLAG;
    }
    frame->can_id = canid;
    frame->can_dlc = (fe_r1 & CAN_TXEFE_1_DLC_Msk) >> CAN_TXEFE_1_DLC_Pos;

    /* timestamp and message marker are currently unprased */
}

static void _rf0n_isr(candev_t *candev)
{
    can_t *dev = container_of(candev, can_t, candev);
    Can* can = dev->conf->can;
    unsigned lvl = 0;
    do {
        uint32_t rx_fifo_status = can->RXF0S.reg;
        size_t rx_get_idx = ((rx_fifo_status & CAN_RXF0S_F0GI_Msk) >> CAN_RXF0S_F0GI_Pos);
        size_t rx_put_idx = ((rx_fifo_status & CAN_RXF0S_F0PI_Msk) >> CAN_RXF0S_F0PI_Pos);
        size_t rx_fifo_lvl =  ((rx_fifo_status & CAN_RXF0S_F0FL_Msk) >> CAN_RXF0S_F0FL_Pos);
        lvl = rx_fifo_lvl;
        DEBUG("rx get index = %u\n", rx_get_idx);
        DEBUG("rx put index = %u\n", rx_put_idx);
        DEBUG("rx fifo lvl = %u\n", rx_fifo_lvl);

        struct can_frame frame = {0};
        /* Reuse variable to avoid multiple read of the same register */
        uint32_t rxfe_0 = dev->msg_ram.rx_fifo_0[rx_get_idx].RXF0E_0.reg;
        uint32_t rxfe_1 = dev->msg_ram.rx_fifo_0[rx_get_idx].RXF0E_1.reg;

        _mcan_hdr_can_frame(&frame, rxfe_0, rxfe_1);
        /* extract extra data here e.g. timestamps */
        memcpy(frame.data, (uint32_t *)dev->msg_ram.rx_fifo_0[rx_get_idx].RXF0E_DATA,
               frame.can_dlc);

        /* acknowledge FIFO */
        can->RXF0A.reg = CAN_RXF0A_F0AI(rx_get_idx);
        if (dev->candev.event_callback) {
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_RX_INDICATION, &frame);
        }
    } while (lvl > 1);
}

static void _rf1n_isr(candev_t *candev)
{
    can_t *dev = container_of(candev, can_t, candev);
    Can* can = dev->conf->can;
    unsigned lvl = 0;
    do {
        uint32_t rx_fifo_status = can->RXF1S.reg;
        size_t rx_get_idx = ((rx_fifo_status & CAN_RXF1S_F1GI_Msk) >> CAN_RXF1S_F1GI_Pos);
        size_t rx_put_idx = ((rx_fifo_status & CAN_RXF1S_F1PI_Msk) >> CAN_RXF1S_F1PI_Pos);
        size_t rx_fifo_lvl = ((rx_fifo_status & CAN_RXF1S_F1FL_Msk) >> CAN_RXF1S_F1FL_Pos);
        lvl = rx_fifo_lvl;
        DEBUG("rx get index = %u\n", rx_get_idx);
        DEBUG("rx put index = %u\n", rx_put_idx);
        DEBUG("rx fifo lvl = %u\n", rx_fifo_lvl);

        struct can_frame frame = {0};
        /* Reuse variable to avoid multiple read of the same register */
        uint32_t rxfe_0 = dev->msg_ram.rx_fifo_1[rx_get_idx].RXF1E_0.reg;
        uint32_t rxfe_1 = dev->msg_ram.rx_fifo_1[rx_get_idx].RXF1E_1.reg;

        _mcan_hdr_can_frame(&frame, rxfe_0, rxfe_1);
        /* extract extra data here e.g. timestamps */
        memcpy(frame.data, (uint32_t *)dev->msg_ram.rx_fifo_1[rx_get_idx].RXF1E_DATA,
               frame.can_dlc);

        /* acknowledge FIFO */
        can->RXF1A.reg = CAN_RXF1A_F1AI(rx_get_idx);
        if (dev->candev.event_callback) {
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_RX_INDICATION, &frame);
        }
    } while (lvl > 1);
}

static void _tefn_isr(candev_t *candev)
{
    can_t *dev = container_of(candev, can_t, candev);
    Can* can = dev->conf->can;

    unsigned lvl = 0;
    do {
        uint32_t txfs = can->TXEFS.reg;
        size_t tx_get_idx = ((txfs & CAN_TXEFS_EFGI_Msk) >> CAN_TXEFS_EFGI_Pos);
        size_t tx_put_idx = ((txfs & CAN_TXEFS_EFPI_Msk) >> CAN_TXEFS_EFPI_Pos);
        size_t tx_fifo_lvl =  ((txfs & CAN_TXEFS_EFFL_Msk) >> CAN_TXEFS_EFFL_Pos);
        lvl = tx_fifo_lvl;
        DEBUG("tx get index = %u\n", tx_get_idx);
        DEBUG("tx put index = %u\n", tx_put_idx);
        DEBUG("tx fifo lvl = %u\n", tx_fifo_lvl);

        size_t idx = tx_get_idx;

        struct can_frame  frame = {};

        uint32_t txfe_0 = dev->msg_ram.tx_event_fifo[idx].TXEFE_0.reg;
        uint32_t txfe_1 = dev->msg_ram.tx_event_fifo[idx].TXEFE_1.reg;

        _mcan_hdr_can_frame(&frame, txfe_0, txfe_1);
        /* extract extra data here e.g. timestamps */
        memcpy(frame.data, (uint32_t *)dev->msg_ram.tx_buffer[idx].TXBE_DATA, frame.can_dlc);

        /* acknowledge TX EVENT */
        can->TXEFA.reg = CAN_TXEFA_EFAI(idx);
        if (dev->candev.event_callback) {
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_TX_CONFIRMATION, &frame);
        }
    } while (lvl > 1);
}

/* error code handling for LEC and DLEC errorS */
static void _error_code_handling(candev_t *candev, uint8_t error_code)
{
    can_t *dev = container_of(candev, can_t, candev);

    switch (error_code) {
        case CANDEV_SAMD5X_NO_ERROR:
            /* no error detected with in last can event */
        case CANDEV_SAMD5X_NO_CHANGE_ERROR:
            /* no can event happened -> unchanged */
            break;
        case CANDEV_SAMD5X_STUFF_ERROR:
            DEBUG_PUTS("STUFF error");
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_RX_ERROR, NULL);
            break;
        case CANDEV_SAMD5X_FORM_ERROR:
            DEBUG_PUTS("FORM error");
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_RX_ERROR, NULL);
            break;
        case CANDEV_SAMD5X_ACK_ERROR:
            DEBUG_PUTS("ACK error");
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_TX_ERROR, NULL);
            break;
        case CANDEV_SAMD5X_BIT1_ERROR:
            DEBUG_PUTS("BIT1 error");
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_TX_ERROR, NULL);
            break;
        case CANDEV_SAMD5X_BIT0_ERROR:
            DEBUG_PUTS("BIT0 error");
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_TX_ERROR, NULL);
            break;
        case CANDEV_SAMD5X_CRC_ERROR:
            DEBUG_PUTS("CRC error");
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_RX_ERROR, NULL);
            break;
        default:
            break;
    }
}

static void _error_count_handling(candev_t *candev)
{
    can_t *dev = container_of(candev, can_t, candev);
    Can* can = dev->conf->can;

    /* Extract the Tx and Rx error counters */
    uint32_t ecr = can->ECR.reg;
    uint8_t tx_err_cnt = (uint8_t)((ecr & CAN_ECR_TEC_Msk) >> CAN_ECR_TEC_Pos);
    uint8_t rx_err_cnt = (uint8_t)((ecr & CAN_ECR_REC_Msk) >> CAN_ECR_REC_Pos);
    DEBUG("tx error counter = %u\n", tx_err_cnt);
    DEBUG("rx error counter = %u\n", rx_err_cnt);

    /* TX count update when TX error count changed (usually ++) */
    if (tx_err_cnt != dev->state.tx_error_count) {
        dev->state.tx_error_count = tx_err_cnt;
        DEBUG_PUTS("tx error count changed");
    }

    /* RX countupdate when RX error count changed (usually ++) */
    if (rx_err_cnt != dev->state.rx_error_count) {
        dev->state.rx_error_count = rx_err_cnt;
        DEBUG_PUTS("rx error count changed");
    }
}

/* PED and PEA must be handled at the same time as reading PSR destroys the LEC and DLEC */
static void _pe_isr(candev_t *candev, uint32_t irq_reg ){
    can_t *dev = container_of(candev, can_t, candev);
    Can* can = dev->conf->can;

    _error_count_handling(candev);

    /* PSR read will set DLEC and LEC to NO_CHANGE */
    uint32_t psr = can->PSR.reg;
    uint8_t lec = (uint8_t)(psr& CAN_PSR_LEC_Msk) >> CAN_PSR_LEC_Pos;
    uint8_t dlec = (uint8_t)(psr & CAN_PSR_DLEC_Msk) >> CAN_PSR_DLEC_Pos;

    /* LEC is not NO_CHANGE when retrieved -> save error_code */
    if (lec != CANDEV_SAMD5X_NO_CHANGE_ERROR) {
       dev->state.last_error_code = lec;
    }

    /* DLEC is not NO_CHANGE when retrieved -> save error_code */
    if (dlec != CANDEV_SAMD5X_NO_CHANGE_ERROR) {
       dev->state.d_last_error_code = dlec;
    }

    /* Interrupt triggered due to protocol error
     * in arbitration phase or data phase without BRS */
    if (irq_reg & CAN_IR_PEA) {
        DEBUG_PUTS("protocol error in arbitration phase");
        _error_code_handling(candev, dev->state.last_error_code);
   }

   /* Interrupt triggered due to protocol error
    * in data phase only CAN_FD with BRS (Bit Rate Switch) */
    if (irq_reg & CAN_IR_PED) {
        DEBUG_PUTS("protocol error in data phase");
        _error_code_handling(candev, dev->state.d_last_error_code);
    }
}

static void _isr(candev_t *candev)
{
    can_t *dev = container_of(candev, can_t, candev);
    Can* can = dev->conf->can;

    uint32_t irq_reg = can->IR.reg;
    DEBUG("isr: IR reg = 0x%08" PRIx32 "\n", irq_reg);

    /* Interrupt triggered due to reception of CAN frame on Rx_FIFO_0 */
    if (irq_reg & CAN_IR_RF0N) {
        DEBUG_PUTS("New message in Rx FIFO 0");
        /* Clear the interrupt source flag */
        can->IR.reg |= CAN_IR_RF0N;
        _rf0n_isr(candev);
    }

    /* Interrupt triggered due to reception of CAN frame on Rx_FIFO_1 */
    if (irq_reg & CAN_IR_RF1N) {
        DEBUG_PUTS("New message in Rx FIFO 1");
        /* Clear the interrupt source flag */
        can->IR.reg |= CAN_IR_RF1N;
        _rf1n_isr(candev);
    }

    /* Interrupt triggered due to new transmission event */
    if (irq_reg & CAN_IR_TEFN) {
        DEBUG_PUTS("New Tx event FIFO entry");
        can->IR.reg |= CAN_IR_TEFN;
        _tefn_isr(candev);
    }

    /* Interrupt triggered due to protocol error in arbitration phase or
     * Interrupt triggered due to protocol error in data phase with BRS (can-fd only)
     * handling reads PSR -> one handler for both IRQ */
    if (irq_reg & (CAN_IR_PEA|CAN_IR_PED)  ) {
        DEBUG_PUTS("protocol error in arbitration phase");
        can->IR.reg |= (irq_reg & (CAN_IR_PEA|CAN_IR_PED));
        _pe_isr(candev, irq_reg);
   }

    /* Interrupt triggered due to Bus_Off status */
    if (irq_reg & CAN_IR_BO) {
        DEBUG_PUTS("Bus off");
        can->IR.reg |= CAN_IR_BO;
        if (dev->candev.event_callback) {
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_BUS_OFF, NULL);
        }
    }

    /* Interrupt triggered due to Error warning status */
    if (irq_reg & CAN_IR_EW) {
        DEBUG_PUTS("Error warning");
        can->IR.reg |= CAN_IR_EW;
        if (dev->candev.event_callback) {
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_ERROR_WARNING, NULL);
        }
    }

    /* Interrupt triggered due to Error passive status */
    if (irq_reg & CAN_IR_EP) {
        DEBUG_PUTS("Error Passive");
        can->IR.reg |= CAN_IR_EP;
        if (dev->candev.event_callback) {
            dev->candev.event_callback(&(dev->candev), CANDEV_EVENT_ERROR_PASSIVE, NULL);
        }
    }

    /* Enable the peripheral's interrupt */
    if (can == CAN0) {
        NVIC_EnableIRQ(CAN0_IRQn);
    }
    else {
        /* it's safe to assume can == CAN1 */
        NVIC_EnableIRQ(CAN1_IRQn);
    }
}

#ifdef ISR_CAN0
void ISR_CAN0(void)
{
    DEBUG_PUTS("ISR CAN0");

    /* Disable the peripheral's interrupt to avoid potential 'interrupt bouncing' */
    NVIC_DisableIRQ(CAN0_IRQn);
    if (_can_0->candev.event_callback) {
        _can_0->candev.event_callback(&(_can_0->candev), CANDEV_EVENT_ISR, NULL);
    }
    cortexm_isr_end();
}
#endif

#ifdef ISR_CAN1
void ISR_CAN1(void)
{
    DEBUG_PUTS("ISR CAN1");

    /* Disable the peripheral's interrupt to avoid potential 'interrupt bouncing' */
    NVIC_DisableIRQ(CAN1_IRQn);
    if (_can_1->candev.event_callback) {
        _can_1->candev.event_callback(&(_can_1->candev), CANDEV_EVENT_ISR, NULL);
    }
   cortexm_isr_end();
}
#endif
